/*=====================================================================================
 * Description:
 *   C file to define parameter storage instances
 *====================================================================================*/
#include <time.h>
#include <math.h>
#include "mdf_common.h"
#include "mlink.h"
#include "string.h"
#include "freertos/FreeRTOS.h"
#include "freertos/semphr.h"
#include "freertos/queue.h"
#include "esp_log.h"
// #include "modbus_params.h"  // for modbus parameters structures
#include "mbcontroller.h"
#include "sdkconfig.h"
#include "mb_app.h"
#include "device_controller.h"


// Timeout between polls
#define POLL_TIMEOUT_MS                 (1000)
#define POLL_TIMEOUT_TICS               (POLL_TIMEOUT_MS / portTICK_RATE_MS)

extern int write_cid_to_report_queue(subdev_port_t *subdev_port, subdev_cid_t *subdev_cid, int timeout);

static const char *TAG = "mb_app";

// The function to get pointer to parameter storage (instance) according to parameter description table
void* master_get_param_data(void *handle, const mb_parameter_descriptor_t* param_descriptor)
{

    assert(param_descriptor != NULL);
	assert(handle != NULL);
    void* instance_ptr = NULL;
	mb_app_t *mb_handle = handle;
	subdev_port_t *subdev_port = mb_handle->port_data;
	instance_ptr = &subdev_port->cids[param_descriptor->param_offset].value;
    // if (param_descriptor->param_offset != 0) {
	// 	instance_ptr = &mb_handle->data_buf[param_descriptor->param_offset-1];
    // } else {
    //     ESP_LOGE(TAG, "Wrong parameter offset for CID #%d", param_descriptor->cid);
    //     assert(instance_ptr != NULL);
    // }
    return instance_ptr;
}

void* master_get_cid_info(void *handle, const mb_parameter_descriptor_t* param_descriptor)
{
    assert(param_descriptor != NULL);
	assert(handle != NULL);

	mb_app_t *mb_handle = handle;
	subdev_port_t *subdev_port = mb_handle->port_data;
	if(subdev_port){
		if(subdev_port->cids){
			return &subdev_port->cids[param_descriptor->param_offset];
		}
	}
	return NULL;
}

static uint32_t mb_ntohl(uint32_t val)
{
	uint32_t ret=0;
	ret = (val >> 16) | (val << 16);
	return ret;	
}

uint64_t mb_ntohll(uint64_t val)
{
    return (((uint64_t) mb_ntohl(val)) << 32) + mb_ntohl(val >> 32);
}
#define mb_swap32(A) ((((unsigned int)(A) & 0xffff0000) >> 16) | (((unsigned int)(A) & 0x0000ffff) << 16))

// User operation function to read slave values and check alarm
void mb_app_task(void *arg)
{
	assert(arg);
	mb_app_t *mb_app_handle = arg;
	subdev_port_t *subdev_port = mb_app_handle->port_data;
	void* master_interface_ptr = mb_app_handle->master_handler;
    esp_err_t err = ESP_OK;
	union {
		uint32_t value_u32;
		float value_float;
	}value;
    const mb_parameter_descriptor_t* param_descriptor = NULL;
	mb_write_msg_t msg;

    ESP_LOGD(TAG, "Start modbus read...");
	while (1) {

		// Read all found characteristics from slave(s)
		for (uint16_t cid = 0; (err != ESP_ERR_NOT_FOUND) && cid < mb_app_handle->parameter_num; cid++)
		{
			// 是否有数据需要写入
			if( xQueueReceive(mb_app_handle->queue_write, &msg, pdMS_TO_TICKS(100)) ){
			
				const mb_parameter_descriptor_t* param_descriptor = NULL;
				mbc_master_init_iface(master_interface_ptr);
				err = mbc_master_get_cid_info(msg.cid, &param_descriptor);
				if ((err != ESP_ERR_NOT_FOUND) && (param_descriptor != NULL)) {
					uint8_t type;
					if(param_descriptor->mb_size == 2) {
						msg.val.value_u32 = mb_ntohl(msg.val.value_u32);
					}
					ESP_LOGI(TAG, "write %"PRIx32"", msg.val.value_u32);
					err = mbc_master_set_parameter(msg.cid, (char*)param_descriptor->param_key,
														(uint8_t*)&msg.val, &type);
					if(err != ESP_OK){
						ESP_LOGD(TAG, "mbc_master_set_parameter ret %d", err);
					}		
				}
				vTaskDelay(pdMS_TO_TICKS(10)); // timeout between polls
			}

			// Get data from parameters description table
			// and use this information to fill the characteristics description table
			// and having all required fields in just one table
			mbc_master_init_iface(master_interface_ptr);
			err = mbc_master_get_cid_info(cid, &param_descriptor);
			if ((err != ESP_ERR_NOT_FOUND) && (param_descriptor != NULL)) {
				subdev_cid_t *subdev_cid= master_get_cid_info(mb_app_handle, param_descriptor);
				if(subdev_cid == NULL){
					ESP_LOGE(TAG, "subdev_cid is NULL");
					continue;
				}
				cid_value_t *val_cache = &subdev_cid->value;
				// void* temp_data_ptr = &subdev_cid->value; //master_get_param_data(mb_app_handle, param_descriptor);
				// assert(temp_data_ptr);
				uint8_t type = 0;

				mbc_master_init_iface(master_interface_ptr);
				err = mbc_master_get_parameter(cid, (char*)param_descriptor->param_key,
													(uint8_t*)&value, &type);
				if (err == ESP_OK) {
					time_t now = time(NULL);
					float val_pre=0, val_now=0;
					subdev_cid->error = 0;
					if ((param_descriptor->param_type == PARAM_TYPE_FLOAT)
					|| (param_descriptor->param_type == PARAM_TYPE_FLOAT_ABCD)
					|| (param_descriptor->param_type == PARAM_TYPE_FLOAT_CDAB)
					|| (param_descriptor->param_type == PARAM_TYPE_FLOAT_BADC)
					|| (param_descriptor->param_type == PARAM_TYPE_FLOAT_DCBA)) {
						
						val_pre = val_cache->value_float;
						value.value_u32 = mb_ntohl(value.value_u32);
						if(subdev_cid->f != 1.0)
							val_cache->value_float = value.value_float * subdev_cid->f;
						else
							val_cache->value_float = value.value_float;
						val_now = val_cache->value_float;

					} else if(param_descriptor->param_type == PARAM_TYPE_U32
					|| param_descriptor->param_type == PARAM_TYPE_U32_ABCD
					|| param_descriptor->param_type == PARAM_TYPE_U32_CDAB
					|| param_descriptor->param_type == PARAM_TYPE_U32_BADC
					|| param_descriptor->param_type == PARAM_TYPE_U32_DCBA) {
						val_pre = val_cache->value_u32;
						value.value_u32 = mb_ntohl(value.value_u32);
						if(subdev_cid->f != 1.0)
							val_cache->value_u32 = value.value_u32 * subdev_cid->f;
						else 
							val_cache->value_u32 = value.value_u32;
						val_now = val_cache->value_u32;

					} else if(param_descriptor->param_type == PARAM_TYPE_U16
					|| param_descriptor->param_type == PARAM_TYPE_U16_AB
					|| param_descriptor->param_type == PARAM_TYPE_U16_BA) {
						// val_pre = *(uint16_t*)temp_data_ptr;
						val_pre = val_cache->value_u16;
						// coil or discrete 
						if(param_descriptor->mb_param_type == MB_PARAM_COIL
						|| param_descriptor->mb_param_type == MB_PARAM_DISCRETE) {
							value.value_u32 = value.value_u32&(1UL<<(param_descriptor->mb_reg_start%8)) ? 1 : 0;
							val_cache->value_u16 = value.value_u32;
						}else{
				
							if(subdev_cid->f != 1.0)
								val_cache->value_u16 = value.value_u32 * subdev_cid->f;
							else 
								val_cache->value_u16 = value.value_u32;
						}
						val_now = val_cache->value_u16;
					} else {
						memcpy((void*)val_cache, &value, param_descriptor->param_size);
					}
					subdev_cid->update_time = now;
					ESP_LOGD(TAG, "Characteristic #%"PRIu16" %s (%s) value = 0x%"PRIx32" read successful.",
									param_descriptor->cid,
									(char*)param_descriptor->param_key,
									(char*)param_descriptor->param_units,
									*(uint32_t*)val_cache);

					// 变换上报处理
					if(subdev_cid->change_val > 0){
						if(fabsf(val_pre - val_now) >= subdev_cid->change_val){
							ESP_LOGV(TAG, "chage value %f %f %f", val_pre, val_now, subdev_cid->change_val);
							// report cid
							subdev_cid->report_time = now;
							write_cid_to_report_queue(subdev_port, subdev_cid, pdMS_TO_TICKS(100));
						}
					}
					// 周期上报处理
					if(subdev_cid->max_inr > 0){
						if(now - subdev_cid->report_time >= subdev_cid->max_inr){
							ESP_LOGV(TAG, "period value %lld %lld %"PRIu16, now, subdev_cid->report_time, subdev_cid->max_inr);
							subdev_cid->report_time = now;
							write_cid_to_report_queue(subdev_port, subdev_cid, pdMS_TO_TICKS(100));
						}
					}
				} else {
					subdev_cid->error = 1;
					ESP_LOGI(TAG, "Characteristic #%"PRIu16" (%s) read fail, err = 0x%d (%s).",
										param_descriptor->cid,
										(char*)param_descriptor->param_key,
										(int)err,
										(char*)esp_err_to_name(err));
				}
				
			}// end if ((err != ESP_ERR_NOT_FOUND) && (param_descriptor != NULL))

			vTaskDelay(pdMS_TO_TICKS(subdev_port->poll_interval)); // timeout between polls
		}// end for (uint16_t cid = 0; (err != ESP_ERR_NOT_FOUND) && cid < MASTER_MAX_CIDS; cid++)
		mdf_event_loop_send(MDF_EVENT_MLINK_SET_STATUS, NULL);
		vTaskDelay(pdMS_TO_TICKS(10)); // timeout between polls
	}

}

esp_err_t mb_app_write(void *handle, mb_write_msg_t *msg)
{
	if(handle == NULL || msg == NULL){
		return ESP_ERR_INVALID_ARG;
	}
	mb_app_t *mb_app_handle = handle;
	assert(mb_app_handle->queue_write);

	if(xQueueSend(mb_app_handle->queue_write, msg, pdMS_TO_TICKS(100))){
		return ESP_OK;
	}
	return ESP_FAIL;
}

esp_err_t mb_app_read_word(void *handle, uint8_t cid, uint16_t *val)
{
	assert(handle);
	mb_app_t *mb_app_handle = handle;
	void* master_interface_ptr = mb_app_handle->master_handler;
	assert(master_interface_ptr);

	const mb_parameter_descriptor_t* param_descriptor = NULL;
	mbc_master_init_iface(master_interface_ptr);
	int ret = mbc_master_get_cid_info(cid, &param_descriptor);
	if ((ret != ESP_ERR_NOT_FOUND) && (param_descriptor != NULL)) {
		void* temp_data_ptr = master_get_param_data(mb_app_handle, param_descriptor);
		*val = (*(uint16_t *)temp_data_ptr);
		return ESP_OK;
	}
	return ESP_FAIL;
}

esp_err_t mb_app_read_float(void *handle, uint8_t cid, float *val)
{
	assert(handle);
	mb_app_t *mb_app_handle = handle;
	void* master_interface_ptr = mb_app_handle->master_handler;
	assert(master_interface_ptr);

	const mb_parameter_descriptor_t* param_descriptor = NULL;
	mbc_master_init_iface(master_interface_ptr);
	int ret = mbc_master_get_cid_info(cid, &param_descriptor);
	if ((ret != ESP_ERR_NOT_FOUND) && (param_descriptor != NULL)) {
		void* temp_data_ptr = master_get_param_data(mb_app_handle, param_descriptor);
		*val = (float)(*(uint32_t *)temp_data_ptr);
		return ESP_OK;
	}
	return ESP_FAIL;
}



esp_err_t mb_app_set_descriptor(void *handle)
{
	mb_app_t *mb_handle = handle;

    int err = mbc_master_set_descriptor(mb_handle->parameter_descriptor, mb_handle->parameter_num);
    MB_RETURN_ON_FALSE((err == ESP_OK), ESP_ERR_INVALID_STATE, TAG,
                                "mb controller set descriptor fail, returns(0x%x).",
                                (unsigned int)err);
    ESP_LOGI(TAG, "Modbus master stack initialized...");
	return err;
}

// Modbus master initialization
esp_err_t mb_master_serial_create(mb_app_config_t * cfg, mb_app_t *mb_handle)
{
	mb_handle->queue_write = xQueueCreate( 3, sizeof( mb_write_msg_t ) );

    // Initialize and start Modbus controller
    // mb_handle->comm.port = cfg->uart_num;
    // mb_handle->comm.mode = MB_MODE_RTU;
    // mb_handle->comm.baudrate = cfg->baud_rate;
    // mb_handle->comm.parity = cfg->parity;

    esp_err_t err = mbc_master_init(MB_PORT_SERIAL_MASTER, &mb_handle->master_handler);
    MB_RETURN_ON_FALSE((mb_handle->master_handler != NULL), ESP_ERR_INVALID_STATE, TAG,
                                "mb controller initialization fail.");
    MB_RETURN_ON_FALSE((err == ESP_OK), ESP_ERR_INVALID_STATE, TAG,
                            "mb controller initialization fail, returns(0x%x).",
                            (unsigned int)err);
    err = mbc_master_setup((void*)&mb_handle->comm);
    MB_RETURN_ON_FALSE((err == ESP_OK), ESP_ERR_INVALID_STATE, TAG,
                            "mb controller setup fail, returns(0x%x).",
                            (unsigned int)err);

    // Set UART pin numbers
    err = uart_set_pin(cfg->uart_num, cfg->tx_io_num, cfg->rx_io_num,
                              cfg->rts_io_num, UART_PIN_NO_CHANGE);
    MB_RETURN_ON_FALSE((err == ESP_OK), ESP_ERR_INVALID_STATE, TAG,
            "mb serial set pin failure, uart_set_pin() returned (0x%x).", (unsigned int)err);

    err = mbc_master_start();
    MB_RETURN_ON_FALSE((err == ESP_OK), ESP_ERR_INVALID_STATE, TAG,
                            "mb controller start fail, returns(0x%x).",
                            (unsigned int)err);

    // Set driver mode to Half Duplex
    err = uart_set_mode(cfg->uart_num, UART_MODE_RS485_HALF_DUPLEX);
    MB_RETURN_ON_FALSE((err == ESP_OK), ESP_ERR_INVALID_STATE, TAG,
            "mb serial set mode failure, uart_set_mode() returned (0x%x).", (unsigned int)err);
    // Set read timeout of UART TOUT feature
    ESP_ERROR_CHECK(uart_set_rx_timeout(cfg->uart_num, 3)); // 3.5T * 8 = 28 ticks, TOUT=3 -> ~24..33 ticks

    vTaskDelay(5);
    err = mbc_master_set_descriptor(mb_handle->parameter_descriptor, mb_handle->parameter_num);
    MB_RETURN_ON_FALSE((err == ESP_OK), ESP_ERR_INVALID_STATE, TAG,
                               "mb controller set descriptor fail, returns(0x%x).",
                               (unsigned int)err);
    ESP_LOGI(TAG, "Modbus master stack initialized...");
    return err;
}

// Modbus master initialization
esp_err_t mb_master_tcp_create(mb_app_t *mb_handle)
{
	mb_handle->queue_write = xQueueCreate( 3, sizeof( mb_write_msg_t ) );

    esp_err_t err = mbc_master_init_tcp(&mb_handle->master_handler);
    MB_RETURN_ON_FALSE((mb_handle->master_handler != NULL), ESP_ERR_INVALID_STATE,
                                TAG,
                                "mb controller initialization fail.");
    MB_RETURN_ON_FALSE((err == ESP_OK), ESP_ERR_INVALID_STATE,
                            TAG,
                            "mb controller initialization fail, returns(0x%x).",
                            (unsigned int)err);

    err = mbc_master_setup((void*)&mb_handle->comm);
    MB_RETURN_ON_FALSE((err == ESP_OK), ESP_ERR_INVALID_STATE,
                            TAG,
                            "mb controller setup fail, returns(0x%x).",
                            (unsigned int)err);

    err = mbc_master_set_descriptor(mb_handle->parameter_descriptor, mb_handle->parameter_num);
    MB_RETURN_ON_FALSE((err == ESP_OK), ESP_ERR_INVALID_STATE,
                                TAG,
                                "mb controller set descriptor fail, returns(0x%x).",
                                (unsigned int)err);
    ESP_LOGI(TAG, "Modbus master stack initialized...");

    err = mbc_master_start();
    MB_RETURN_ON_FALSE((err == ESP_OK), ESP_ERR_INVALID_STATE,
                            TAG,
                            "mb controller start fail, returns(0x%x).",
                            (unsigned int)err);
    vTaskDelay(5);
    return err;
}